#include <cmath>
#include <string>
#include <GL/gl.h>
#include <GL/glu.h>

#include <SRL/Utils/Vector.h>
#include <SRL/Engine/Camera.h>

namespace srl {
    
    Camera::Camera() {
        Position.x = 5;
        Position.y = 1;
        Position.z = 2;
        
        Target.x = 0;
        Target.y = 0;
        Target.z = 0;
    }
    Camera::Camera(srl::Vector<float> p) {
    	Position = p;
    }
    Camera::Camera(srl::Vector<float> p, srl::Vector<float> t) {
    	Position = p;
    	Target = t;
    }
    Camera::Camera(const srl::Camera &c) {
        angleH = c.angleH;
        angleV = c.angleV;
        walkSpeed = c.walkSpeed;
        runSpeed = c.runSpeed;
        turnSpeed = c.turnSpeed;
        visionField = c.visionField;
        
        Target = c.Target;
        Position = c.Position;
    }
    
    /** Init Object with the speed wanted ****/
    void Camera::Init(int p_fovy, double p_ratio, double p_near, double p_far, float p_visionField, float p_walkSpeed, float p_runSpeed, int p_turnSpeed) {
        fovy = p_fovy;
        ratio = p_ratio;
        m_near = p_near;
        m_far = p_far;
        
        walkSpeed = p_walkSpeed;
        runSpeed = p_runSpeed;
        turnSpeed = p_turnSpeed;
        visionField = p_visionField;
        
        angleH = 0;
        angleV = 0;
    }
    
    /** Place the camera ****/
    void Camera::Look() {
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        gluLookAt(Position.x, Position.y, Position.z, Target.x, Target.y, Target.z, 0, 1, 0 );
    }
    void Camera::Perspective() {
        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        gluPerspective(90, 800.0/600.0, 0.1, 1000);
    }
    
    /** Control Position Camera ****/
    void Camera::Back() {
        Position.z -= cos(angleH) * walkSpeed;
        Position.x -= sin(angleH) * walkSpeed;
        calculTarget();
    }
    void Camera::Walk() {
        Position.z += cos(angleH) * walkSpeed;
        Position.x += sin(angleH) * walkSpeed;
        
        calculTarget();
    }
    void Camera::Run() {
        Position.z += cos(angleH) * runSpeed;
        Position.x += sin(angleH) * runSpeed;
        
        calculTarget();
    }
    void Camera::Right() {
        Position.z += sin(angleH) * walkSpeed;
        Position.x -= cos(angleH) * walkSpeed;
        
        calculTarget();
    }
    void Camera::Left() {
        Position.z -= sin(angleH) * walkSpeed;
        Position.x += cos(angleH) * walkSpeed;
        
        calculTarget();
    }
    
    /** Control Target Camera ****/
    void Camera::TargetMove(int x, int y) {
        
        float todecal = (double)6.2831/turnSpeed;
        
        if(x>0)
            for(int xcal = 0 ; xcal < x ; xcal++) { angleH -= todecal; }
        else
            for(int xcal = 0 ; xcal > x ; xcal--) { angleH += todecal; }
                
        if(y>0)
            for(int ycal = 0 ; ycal < y ; ycal++) { angleV -= todecal; }
        else
            for(int ycal = 0 ; ycal > y ; ycal--) { angleV += todecal; }
            
        
        if(angleH<0)
            angleH = 6.2831 + angleH;
        if(angleV<0)
            angleV = 0;
            
        if(angleH>6.2831)
            angleH = angleH - 6.2831;
        if(angleV>3.1415)
            angleV = 3.1415;
             
        calculTarget();
    }
    
    void Camera::calculTarget() {
        Target.x = Position.x + sin(angleH) * visionField;
        Target.y = Position.y + sin(angleV-1.5707) * visionField;
        Target.z = Position.z + cos(angleH) * visionField;
    }
    
    
    srl::Vector<float> Camera::GetPos() {
        return srl::Vector<float>(Position.x, Position.y, Position.z, 1.0);
    }
    srl::Vector<float> Camera::GetTar() {
        return Target;
    }
}
